
#include <algorithm>
#include "deformationSkelMesh.h"
#include "weightsScheme.h"

//~ namespace MLS {

DeformationSkelMesh::DeformationSkelMesh() {
   
   DeformationMesh::DeformationMesh();
   model = 0;
   VB_WeightsTable = 0;
   DistancesTable = 0;
   
   anglebone5 = 0;
   numframe = 0;
}

DeformationSkelMesh::DeformationSkelMesh(MeshWithSkeleton *m) {
   
   DeformationMesh::DeformationMesh();
   
   assert(m != 0);
   assert(m->skeleton != 0);
   model = m;
   VB_WeightsTable = 0;
   DistancesTable = 0;
   
   anglebone5 = 0;
   numframe = 0;
}

DeformationSkelMesh::~DeformationSkelMesh() {
   
   if (VB_WeightsTable != 0) delete []VB_WeightsTable;
   if (DistancesTable != 0) delete []DistancesTable;
}

void DeformationSkelMesh::setMesh(MeshWithSkeleton *m) {
   
   assert(m != 0);
   model = m;
}
   
void DeformationSkelMesh::setting(const Constraints *constraints) {
   
   assert(model != 0);
   assert(model->skeleton != 0);
   
   ipositions.resize(model->nvertices());
   inormals.resize(model->nvertices());
   Vertex_iterator vi = model->vertices_begin();
   for (unsigned int i=0; i<ipositions.size(); ++i, ++vi) {
      ipositions[i] = vi->point();
      inormals[i] = vi->normal();
   }
   
   //~ /// 
   //~ Deformation::setting(model->i_vpositions, model->nvertices(), &model->skeleton->joints);
   //~ // Compute the weights table
   //~ if (WeightsTable != 0) delete []WeightsTable;
   //~ WeightsTable = MLS::compute_WeightsTable(model, model->skeleton, constraints);
   //~ Deformation::initialize();
   
   cout<<"aaaaa"<<endl;
   /// 
   Deformation::setting(&model->skeleton->joints, 
                         model->skeleton->nJoints(), &constraints->positions);
   cout<<"bbbb"<<endl;
   // Compute the weights table
   if (WeightsTable != 0) delete []WeightsTable;
   unsigned int nminusone = std::count(constraints->ids.begin(), constraints->ids.end(), -1);
   if (nminusone == 0) { // If all constraints are associated with joints
      cout<<"if nminusone == 0"<<endl;
      WeightsTable = ::compute_WeightsTable(model, constraints);
      DistancesTable = ::compute_DistancesTable(model, constraints);
   }
   else if (nminusone < constraints->ids.size()) { // If constraints are represented a trihedron
      cout<<"else if nminusone < constraints->ids.size()"<<endl;
      WeightsTable = ::compute_WeightsTable_Trihedrons(model, constraints);
   }
   
   cout<<"cccc"<<endl;
   VB_WeightsTable = ::compute_WeightsTable2(model, constraints);
   cout<<"dddd"<<endl;
   initialize();
   cout<<"eeeee"<<endl;
   old_angle.clear();
   old_angle.resize(model->skeleton->nBones());
   for (int i=0; i<old_angle.size(); ++i) {
      vector<Number> array(model->skeleton->nBones(), 0.0);
      old_angle[i] = array;
   }
   cout<<"ffffff"<<endl;
   turns.clear();
   turns.resize(model->skeleton->nBones(), 1);
   cout<<"ggggg"<<endl;
   
}

void DeformationSkelMesh::initialize() {
   
   // call the initialize function of the parent class
   Deformation::initialize();
   vhatl_mesh.resize(model->nvertices());
   ul.resize(model->skeleton->nJoints());
   pstarlistv.resize(model->nvertices());
   initialize_pstarlist_model();
   
   ubones.resize(18, Vector3(0,0,0));
   tbones.resize(18, Vector3(0,0,0));
   
   tvectors0.resize(18, Vector3(0,0,0));
   tvectors1.resize(18, Vector3(0,0,0));
   
   tlvectors0.resize(18, Vector3(0,0,0));
   tlvectors1.resize(18, Vector3(0,0,0));
   
   tscalar0.resize(18,0);
   tscalar1.resize(18,0);
}

void DeformationSkelMesh::rotate(const Point3 &o, const Vector3 &u, Number angle) {
   
   Deformation::rotate(o,u,angle);
   
   Number cos_a = cos(angle);
   Number sin_a = sin(angle);
   for (unsigned int i=0; i<vhatl_mesh.size(); ++i) {
      Vector3 &v = vhatl_mesh[i];
      v = v*cos_a + cross_product(u,v)*sin_a + (u*v)*u*(1.0-cos_a);
   }
}

void DeformationSkelMesh::initialize_pstarlist_model() {
   
   unsigned int ii=0;
   Vertex_iterator vi = model->vertices_begin();
   while (vi != model->vertices_end()) {
      Vector3 pstar(0,0,0);
      for (unsigned int j=0; j<vi->jointsweights.size(); ++j) {
         pstar = pstar + (vi->jointsweights[j])*pstarList[j];
      }
      //~ Number *pi = &model->i_vpositions[vi->id()];
      const Point3 &pi = vi->point();
      vhatl_mesh[ii] = Vector3(pi[0]-pstar[0],pi[1]-pstar[1],pi[2]-pstar[2]);
      //~ pstar[0] /= Wt; pstar[1] /= Wt; pstar[2] /= Wt;
      pstar = Vector3(pstar[0]/Wt, pstar[1]/Wt, pstar[2]/Wt);
      //~ wtl[ii] = Wt;
      
      Wt=0.0; pstar=Vector3(0,0,0);
      for (unsigned j=1; j<model->nBones(); ++j) {
         //~ const Point3 &p = model->getVertexPosition(vi);
         const Point3 &p = vi->point();
         const Point3 P = p - model->skeleton->getBoneProjector(&p[0], j);
         W = vi->bonesweights[j];
         Wt += W;
         //~ pstar[0] += W*P[0]; pstar[1] += W*P[1]; pstar[2] += W*P[2];
         pstar = pstar + W*Vector3(P[0],P[1],P[2]);
      }
      pstarlistv[ii] = Point3(pstar[0]/Wt, pstar[1]/Wt, pstar[2]/Wt);
      ++vi; ++ii;
   }
}

void DeformationSkelMesh::computeQuatRotation(const Number v_[], const Number &qs, const Vector3 &qv) {
   
   Vector3 v(v_[0],v_[1],v_[2]);
   Number s = -1.0*(qv*v);
   Vector3 vv = v*qs + cross_product(qv,v);
   Vector3 r = qv*(-s) + vv*qs - cross_product(vv, qv);
   
   rv[0] = r[0];
   rv[1] = r[1];
   rv[2] = r[2];
}

void DeformationSkelMesh::compRotTra_simple(unsigned int i) {
   
   Vertex_iterator vi = model->vertices_begin();
   advance(vi, i);
   Point3 po = model->getVertexPosition(i);
   vector<Point3> pilist(model->nBones(), Point3(0,0,0));
   vector<Point3> qilist(model->nBones(), Point3(0,0,0));
   
   // build the control point associated with the vertex i
   for (unsigned int j=1; j<model->nBones(); ++j) {
      const Number &t = vi->ts[j];
      //~ pilist[j] = model->i_skeleton->getPointOnBone(j, t);
      //~ qilist[j] = model->skeleton->getPointOnBone(j, t);
      pilist[j] = model->i_skeleton->getEllipPointOnBone(j, t);
      qilist[j] = model->skeleton->getEllipPointOnBone(j, t);
      //~ cout<<"   pi: "<<pilist[j]<<" qi "<<qilist[j]<<endl;
   }
   
   // Compute centroids pstar and qstar using the pi and qi computed before
   Wt=pstar[0]=pstar[1]=pstar[2]=0.0; 
   qstar[0]=qstar[1]=qstar[2]=0.0;
   for (unsigned int j=1; j<model->nBones(); ++j) {
      const Point3 &p = pilist[j];
      const Point3 &q = qilist[j];
      //~ const Vector3 vd = po - p;
      //~ Number d = sqrt(vd*vd);
      //~ if (d == 0) d = 1e-8;
      //~ W = 1.0/(pow(d,2));
      //~ Wt += W;
      W = weight2(i,j);
      Wt += W;
      pstar[0] += W*p[0]; pstar[1] += W*p[1]; pstar[2] += W*p[2];
      qstar[0] += W*q[0]; qstar[1] += W*q[1]; qstar[2] += W*q[2];
   }
   pstar[0] /= Wt; pstar[1] /= Wt; pstar[2] /= Wt;
   qstar[0] /= Wt; qstar[1] /= Wt; qstar[2] /= Wt;
   //~ cout<<"   p*: "<<pstar<<" q* "<<qstar<<endl;
   
   // Compute the rotations vectors for each phat <--> qhat
   vector<Vector3> vri(nconstraints, Vector3(0,0,0));
   for (unsigned int j=1; j<model->nBones(); ++j) {
      const Point3 &p = pilist[j];
      const Point3 &q = qilist[j];
      
      Vector3 pjhat = p - Point3(pstar[0],pstar[1],pstar[2]);
      Vector3 qjhat = q - Point3(qstar[0],qstar[1],qstar[2]);
      
      Vector3 pi(pjhat[0], pjhat[1], pjhat[2]);
      Vector3 qi(qjhat[0], qjhat[1], qjhat[2]);
      Number dot = pi*qi;
      //~ if (dot > 1.0) dot = 1.0;
      //~ if (dot <-1.0) dot =-1.0;
      const Number &norpi = sqrt(pi*pi);
      const Number &norqi = sqrt(qi*qi);
      if (norpi < 1e-8) { vri[j] = Vector3(0,0,0); continue; }
      if (norqi < 1e-8) { vri[j] = Vector3(0,0,0); continue; }
      Number costmp = dot/(norpi*norqi);
      //~ cout<<"costmp: "<<costmp<<" "<<dot<<" "<<norpi*norqi<<endl;
      if (costmp > 1.0) costmp = 1.0;
      if (costmp <-1.0) costmp =-1.0;
      const Number &angle = acos(costmp);
      Vector3 axis = cross_product(pi, qi);
      if (axis*axis < 1e-8) { vri[j] = Vector3(0,0,0); continue; }
      axis = axis/sqrt(axis*axis);
      vri[j] = axis*angle;
      //~ cout<<"vri[j]: "<<vri[j][0]<<" "<<vri[j][1]<<" "<<vri[j][2]<<endl;
   }
   
   // Blend the rotations of each pair phat and qhat
   Number swi = 0.0;
   Vector3 swri(0,0,0);
   for (unsigned int j=1; j<model->nBones(); ++j) {
      const Point3 &p = pilist[j];
      const Point3 &q = qilist[j];
      //~ const Vector3 vd = po - p;
      //~ Number d = sqrt(vd*vd);
      //~ if (d == 0) d = 1e-8;
      //~ W = 1.0/(pow(d,2));
      W = weight2(i,j);
      Vector3 pjhat = p - Point3(pstar[0],pstar[1],pstar[2]);
      
      Vector3 pi(pjhat[0], pjhat[1], pjhat[2]);
      const Number &norpi = sqrt(pi*pi);
      swi  += W*norpi;
      swri = swri + vri[j]*W*norpi;
   }
   Vector3 r;
   if (swi < 1e-8) { r = Vector3(0,0,0); }
   else r = swri/swi;
   
   
   {
   Number angle = sqrt(r*r);
   if (angle < 1e-8) r = Vector3(0,0,1);
   else r = r/angle;
   
   const Number &x=r[0], &y=r[1], &z=r[2];
   Number c = cos(angle);
   Number s = sin(angle);
   Number t = 1.0f - c;
   R(0,0)=t*x*x+c;   R(0,1)=t*x*y-z*s; R(0,2)=t*x*z+y*s; 
   R(1,0)=t*x*y+z*s; R(1,1)=t*y*y+c;   R(1,2)=t*y*z-x*s; 
   R(2,0)=t*x*z-y*s; R(2,1)=t*y*z+x*s; R(2,2)=t*z*z+c; 
   }
   
   /// Deform positions and normals
   vi = model->vertices_begin();
   const Vector3 &pi = vhatList[i];
   VectorCPPL xi(3);
   xi(0) = pi[0]; xi(1) = pi[1]; xi(2) = pi[2];
   VectorCPPL xd = R*xi;
   Point3 &p = vi[i].point();
   p = Point3(xd(0)+qstar[0], xd(1)+qstar[1], xd(2)+qstar[2]);
   
   const Vector3 &ni = inormals[i];
   xi(0) = ni[0]; xi(1) = ni[1]; xi(2) = ni[2];
   xd = R*xi;
   Vector3 &n = vi[i].normal();
   n = Vector3(xd(0),xd(1),xd(2));
}

void DeformationSkelMesh::execute_deformation(const Constraints *constraints) {
   
   //~ cout<<"DeformationSkelMesh::execute_deformation zzzzzzzzzzzzzzzzzzzzzzzz"<<endl;
   //< Deform the skeleton joints
   d_constraintsPos = &constraints->positions; // positions of deformed constraints 
   for (unsigned i=0; i<npositions; ++i) {
      
      compute_qstar_points(i);
      
      computeCorrelationMatrix_points(i);
      eigenvalue = computeLargestRootOfQuarticEquation() + traceM; 
      
      compute_u();
      //~ cout<<" ::> "<<i<<" detM: "<<detM<<"   u: "<<u[0]<<" "<<u[1]<<" "<<u[2]<<" "<<endl;
      
      // Apply the rotation on the vertex position
      computeRotation(&vhatList[i][0]);
      Point3 &p = model->skeleton->joints[i]; //< the vertex positions
      p = Point3(qstar[0]+rv[0], qstar[1]+rv[1], qstar[2]+rv[2]);
      
      qstarList[i] = Vector3(qstar[0],qstar[1],qstar[2]);
      ul[i] = Vector3(u[0], u[1], u[2]);
   }
   
   //~ project_deformation_using_u();
   //~ project_deformation_using_quaternions();
   
   //~ project_deformation_on_mesh_linear();
   project_deformation_on_mesh_std_dualquat(); /// ok
   //~ project_deformation_dualquat_interquat();
   //~ project_deformation_on_mesh_using_simple();
   
   //~ noproject_deformation();
   
   ++numframe;
   
}

void DeformationSkelMesh::project_deformation_on_mesh_using_simple() {
   /*
   unsigned int ii=0, iv=0;
   Vertex_iterator vi = model->vertices_begin();
   while (vi != model->vertices_end()) {
      //~ cout<<"vertice ii: "<<ii<<" : "<<model->getVertexPosition(ii)<<endl;
      compRotTra_simple(ii);
      ++vi; ++ii;
   }
   */
}

inline Number SGN(Number n) { if (n<0) return -1; else return 1;}

Vector3 DeformationSkelMesh::getRotationAroundBoneRep(unsigned int i) {
   
   Skeleton *si = model->i_skeleton; // initial skeleton
   Skeleton *sd = model->skeleton;   // deformed skeleton
   
   const Bone &bone = si->bones[i];   // initial bone
   Vector3 vbone = si->joints[bone.second] - si->joints[bone.first]; // direction vector of initial bone 
   vbone = vbone/sqrt(vbone*vbone); // normalize
   Vector3 vbonerot = sd->joints[bone.second] - sd->joints[bone.first];
   vbonerot = vbonerot/sqrt(vbonerot*vbonerot); // normalize
   
   Number normau = 0;
   Number angle=0, sumangle=0;
   Number sumw=0;
   Number w = 1.0/si->controller[i].size();
   
   for (unsigned int ii=0; ii<si->controller[i].size(); ++ii) {
      
      Vector3 di; Vector3 ddorig;
      int j = si->controller[i][ii];
      if (j>si->bones.size()) {
         j = j-si->bones.size();
         di = si->atbones[j];
         u[0] = ubones[j][0]; u[1] = ubones[j][1]; u[2] = ubones[j][2];
         computeRotation(&di[0]);
         ddorig = Vector3(rv[0], rv[1], rv[2]);
      }
      else {
         const Bone &bi = si->bones[j];   // initial bone
         const Bone &bd = sd->bones[j];   // deformed bone
         di = si->joints[bi.second] - si->joints[bi.first]; // direction vector of initial bone 
         ddorig = sd->joints[bd.second] - sd->joints[bd.first]; // direction vector of deformed bone
      }
      
      //~ u = ubones[i]*(-1.0);
      u[0] = ubones[i][0]*(-1.0); u[1] = ubones[i][1]*(-1.0); u[2] = ubones[i][2]*(-1.0);
      computeRotation(&ddorig[0]);
      Vector3 dd(rv[0], rv[1], rv[2]);
      
      Vector3 unidi = di/sqrt(di*di);
      Vector3 unidd = dd/sqrt(dd*dd);
      
      /// 1er metodo
      Vector3 bisectriz = (unidi+unidd)*0.5;
      Number normab = sqrt(bisectriz*bisectriz);
      
      if (normab != 0) {
         bisectriz = bisectriz*(1.0/normab);
         
         Number pdot = unidi*unidd;
         if (pdot > 1.0) pdot = 1.0;
         if (pdot <-1.0) pdot =-1.0;
         Number angleini = acos(pdot);
         
         Number normav = tan(angleini/2.0);
         Vector3 u0 = cross_product(unidi,unidd);
         u0 = (u0/(sqrt(u0*u0)))*normav;
         Vector3 u1 = cross_product(bisectriz,vbone);
         Vector3 u2 = cross_product(u1,bisectriz);
         Number den = u2*vbone;
         if (abs(den)>1e-3) {
            normau = (u2*u0)/den;
            angle = 2*atan(normau);
         }
         else {
            if (abs(u2*u0) < 1e-3) {
               angle = old_angle[i][j];
               normau = tan(angle/2.0);
            }
            else {
               if (abs(den)<1e-8) {
                  angle = old_angle[i][j];
                  normau = tan(angle/2.0);
                  normau = SGN(normau)*1e8;
               }
               else {
                  normau = (u2*u0)/den;
                  angle = 2*atan(normau);
               }
            }
         }
      }
      else {
         angle = old_angle[i][j];
         normau = tan(angle/2.0);
      }
      
      
      /// ////// 2do metodo
      //~ di = di - vbone*(di*vbone);
      //~ dd = dd - vbone*(dd*vbone);
      //~ Number ndi = sqrt(di*di);
      //~ Number ndd = sqrt(dd*dd);
      
      //~ if (ndi*ndd < 1e-3) {
         //~ angle = old_angle[i][j];
         //~ //continue;
      //~ }
      //~ else {
         //~ di =di/ndi;
         //~ dd =dd/ndd;
         //~ Vector3 dixdd = cross_product(di,dd);
         //~ if ((dixdd * dixdd) <1.0e-3) {
            //~ angle = old_angle[i][j];
         //~ }
         //~ else {
            //~ Number pdot = di *dd;
            //~ if (pdot > 1.0) pdot = 1.0;
            //~ if (pdot < -1.0) pdot = -1.0; 
            //~ angle = acos(pdot);
            //~ if(dixdd *vbone < 0) angle =-angle;
         //~ }
         
      //~ }
      //~ normau = tan(angle/2.0);
      /// //////////// 
      
      /// ///
      //~ Number dif_angle = angle-old_angle[i][j];
      //~ Number turns_dif;
      //~ if (dif_angle < 0) turns_dif = -1;
      //~ else               turns_dif = 0;
      
      //~ turns_dif += (int)(dif_angle/6.2832);
      //~ angle -= turns_dif*6.2832;
      
      //~ if ((angle-old_angle[i][j])>3.1416)
         //~ angle -= 6.2832;
         
      //~ if (numframe>1) {
         //~ if (abs(angle-old_angle[i][j])>0.5)
            //~ angle = old_angle[i][j];
      //~ }
      
      //~ old_angle[i][j] = angle;
      /// ///
      
      if (i==5) {
         if (j==7) w = 0.3;
         else      w = 0.7;
      }
      else if (i==9) {
         if (j==11) w = 0.3;
         else      w = 0.7;
      }
      else 
         w = 1.0/((float)si->controller[i].size());
      
      sumangle +=  w*angle;
      sumw += w;
   }
   
   
   if (sumw==0) { sumangle = 0; }
   else sumangle /= sumw;
   
   //~ if (abs(sumangle) > 6.2832) sumangle -= ((int)(sumangle/6.2832))*6.2832;
   //~ if (sumangle > 3.1416)      sumangle -= 6.2832;
   //~ if (sumangle < -3.1416)     sumangle += 6.2832;
   
   sumangle = sumangle/2.0;
   normau = tan(sumangle);
  
   
   return vbone*normau;
}

void DeformationSkelMesh::getRotationRep(unsigned int i) {
   
   Skeleton *si = model->i_skeleton; // initial skeleton
   Skeleton *sd = model->skeleton;   // deformed skeleton
   const Bone &bi = si->bones[i];   // initial bone
   const Bone &bd = sd->bones[i];   // deformed bone
   
   Vector3 di = si->joints[bi.second] - si->joints[bi.first]; // direction vector of initial bone 
   Vector3 dd = sd->joints[bd.second] - sd->joints[bd.first]; // direction vector of deformed bone 
   
   di = di/sqrt(di*di); // normalize
   dd = dd/sqrt(dd*dd); // normalize
   
   Vector3 bisectriz = (di+dd)*0.5;
   Number normab = sqrt(bisectriz*bisectriz);
   bisectriz = bisectriz/(normab);
   ubones[i] = bisectriz*(ubones[i]*bisectriz);
   
   Vector3 a = cross_product(di,dd);
   if (a*a < 1.0e-4) { comprotaround=false; return; }
   Number pdot = di*dd;
   Number normau;
   if (pdot >= 1.0) { return; }
   a =a/sqrt(a*a);
   if(pdot <= -1.0) normau = 1.0e8;
   else normau = sqrt((1.0-pdot)/(1.0+pdot));
   
   Number transitionlimit = 100;
   Vector3 tmp = a*normau;
   if (tmp*ubones[i] < 0) {
      if (((tmp-ubones[i])*(tmp-ubones[i])) > transitionlimit) turns[i] = -turns[i];
   }
   ubones[i] = ubones[i] + tmp;
   
}

Vector3 DeformationSkelMesh::getTranslationRep(unsigned ib, Vertex_handle v) {
   
   Skeleton *si = model->i_skeleton; // initial skeleton
   Skeleton *sd = model->skeleton;   // deformed skeleton
   
   //~ Number *vp = &model->i_vpositions[v->id()]; // initial vertex position
   //~ Number t;
   //~ Number d = si->getDistance(vp, ib, t);
   
   const Point3 pbi = si->getPointOnBone(ib, v->ts[ib]);
   const Point3 pbd = sd->getPointOnBone(ib, v->ts[ib]);
   return pbd - pbi;
   
}

void DeformationSkelMesh::project_deformation_using_u() {
   
   Number EPSILON = 1.0e-6;
   //< Deform the vertices of the model
   Vertex_iterator vi = model->vertices_begin(); unsigned int ii=0, iv=0;
   while (vi != model->vertices_end()) {
      u[0]=u[1]=u[2]=qstar[0]=qstar[1]=qstar[2]=0.0;
      
      /// Compute u by a linear combination
      for (unsigned int j=0; j<vi->jointsweights.size(); ++j) {
         u[0] += (vi->jointsweights[j])*(ul[j])[0];
         u[1] += (vi->jointsweights[j])*(ul[j])[1];
         u[2] += (vi->jointsweights[j])*(ul[j])[2];
         
         qstar[0] += (vi->jointsweights[j])*qstarList[j][0];
         qstar[1] += (vi->jointsweights[j])*qstarList[j][1];
         qstar[2] += (vi->jointsweights[j])*qstarList[j][2];
      }
      
      
      //~ /// Compute u by a rigorous process
      //~ Vector3 v1 = ul[vi->joints[0]];     Vector3 v2 = v1;
      //~ Number  b1 = vi->jointsweights[0];  Number  b2 = b1;
      //~ qstar[0] = (vi->jointsweights[0])*qstarList[vi->joints[0]][0];
      //~ qstar[1] = (vi->jointsweights[0])*qstarList[vi->joints[0]][1];
      //~ qstar[2] = (vi->jointsweights[0])*qstarList[vi->joints[0]][2];
      //~ for (unsigned int j=1; j<vi->joints.size(); ++j) {
         //~ qstar[0] += (vi->jointsweights[j])*qstarList[vi->joints[j]][0];
         //~ qstar[1] += (vi->jointsweights[j])*qstarList[vi->joints[j]][1];
         //~ qstar[2] += (vi->jointsweights[j])*qstarList[vi->joints[j]][2];
         
         //~ v2[0] = (ul[vi->joints[j]])[0];
         //~ v2[1] = (ul[vi->joints[j]])[1];
         //~ v2[2] = (ul[vi->joints[j]])[2];
         //~ Number sumtmp = b1 + vi->jointsweights[j];
         //~ b1 = b1/sumtmp;
         //~ b2 = (vi->jointsweights[j])/sumtmp;
         //~ // if (sqrt((v2+v1)*(v2+v1)) < sqrt((v2-v1)*(v2-v1))) v2 = (v2)*(-1.0);
         //~ if (v2*v1 < -1) {
            //~ Vector3 w1 = v1/(v1*v1);
            //~ Vector3 w2 = v2/(v2*v2);
            //~ w1 = w1*b1 + w2*b2;
            //~ // if (w1*w1 < 1e-09) v1 = (v1/(sqrt(v1*v1)))*1e9;
            //~ Vector3 uv1 = v1/(sqrt(v1*v1));
            //~ Vector3 uv2 = v2/(sqrt(v2*v2));
            //~ if (w1*w1 < 1e-09 and uv1*uv2 < (-1+EPSILON))   v1 = (v1/(sqrt(v1*v1)))*1e9;
            //~ else                                            v1 = w1/(w1*w1);
         //~ }
         //~ else {
            //~ v1 = v1*b1 + v2*b2;
         //~ }
         //~ b1 = sumtmp;
      //~ }
      //~ u[0] = v1[0]; u[1] = v1[1]; u[2] = v1[2];
      
      
      // Rotate the vertex position
      computeRotation(&vhatl_mesh[ii][0]);
      //~ computeQuatRotation(&vhatl_mesh[ii][0], qs1, qv1);
      Point3 &p = vi->point();
      p = Point3(qstar[0]+rv[0], qstar[1]+rv[1], qstar[2]+rv[2]);
      
      // Rotate the normal vector
      computeRotation(&inormals[ii][0]);
      Vector3 &n = vi->normal();
      n = Vector3(rv[0], rv[1], rv[2]);
      
      ++vi; ++ii; iv+=3;
   }
   
}

void DeformationSkelMesh::project_deformation_using_quaternions() {
   
   Vertex_iterator vi = model->vertices_begin(); unsigned int ii=0, iv=0;
   while (vi != model->vertices_end()) {
      u[0]=u[1]=u[2]=qstar[0]=qstar[1]=qstar[2]=0.0;
      
      /// Compute u using quaternios
      Number  b1 = vi->jointsweights[0]; 
      Number  qs1 = sqrt(1.0/(1.0+(ul[vi->rigging_group])*(ul[vi->rigging_group])));
      Vector3 qv1 = (ul[vi->rigging_group])*qs1;
      Number  b2 = b1;
      Number  qs2 = qs1;
      Vector3 qv2 = qv1;
      
      qstar[0] = (vi->jointsweights[0])*qstarList[vi->rigging_group][0];
      qstar[1] = (vi->jointsweights[0])*qstarList[vi->rigging_group][1];
      qstar[2] = (vi->jointsweights[0])*qstarList[vi->rigging_group][2];
      for (unsigned int j=1; j<vi->jointsweights.size(); ++j) {
         
         qstar[0] += (vi->jointsweights[j])*qstarList[j][0];
         qstar[1] += (vi->jointsweights[j])*qstarList[j][1];
         qstar[2] += (vi->jointsweights[j])*qstarList[j][2];
         
         qs2 = sqrt(1.0/(1.0+(ul[j])*(ul[j])));
         qv2 = (ul[j])*qs2;
         Number sumtmp = b1 + vi->jointsweights[j];
         b1 = b1/sumtmp;
         b2 = (vi->jointsweights[j])/sumtmp;
         Number cosangle = qs2*qs1 + qv2*qv1;
         if (abs(cosangle) > 1.0-EPSILON) {
            b1 = sumtmp; continue;
         }
         if (cosangle < 0) {
            qs2 = -qs2;
            qv2 = qv2*(-1.0);
            cosangle = -cosangle;
         }
         Number angle = acos(cosangle);
         Number sinangle = sin(angle);
         Number coef1 = (1.0/sinangle)*sin(b1*angle);
         Number coef2 = (1.0/sinangle)*sin(b2*angle);
         qs1 = qs1*coef1 + qs2*coef2;
         qv1 = qv1*coef1 + qv2*coef2;
         b1 = sumtmp;
         
      }
      //~ cout<<"-> "<<qs1*qs1 + qv1*qv1<<endl;
      Vector3 o = qv1*(1.0/qs1);
      u[0] = o[0]; u[1] = o[1]; u[2] = o[2];
      
      
      //~ Number  qs1 = sqrt(1.0/(1.0+u*u));
      //~ Vector3 qv1 = u*qs1;
      
      //~ cout<<"q: "<<qs1<<" "<<qv1[0]<<" "<<qv1[1]<<" "<<qv1[2]<<endl;
      //~ cout<<"q: "<<qs1<<endl;
      
      // Rotate the vertex position
      //~ computeRotation(&vhatl_mesh[ii][0]);
      computeQuatRotation(&vhatl_mesh[ii][0], qs1, qv1);
      Point3 &p = vi->point();
      p = Point3(qstar[0]+rv[0], qstar[1]+rv[1], qstar[2]+rv[2]);
      
      // Rotate the normal vector
      computeRotation(&inormals[ii][0]);
      Vector3 &n = vi->normal();
      n = Vector3(rv[0], rv[1], rv[2]);
      ++vi; ++ii; iv+=3;
   }
   
}

void DeformationSkelMesh::project_deformation_dualquat_interquat() {
   
   //~ cout<<"beginnnnnnnnnnnnnnnn"<<endl;
   
   //< Compute the transformation for skeleton bones 
   //< skip the first bone
   ubones[0] = Vector3(0,0,0);
   tvectors0[0] = Vector3(0,0,0);
   tvectors1[0] = Vector3(0,0,0);
   tscalar0[0] = 0;
   tscalar1[0] = 0;
   for (unsigned int i=1; i<model->nBones(); ++i) {
      
      //~ cout<<"i: "<<i<<endl;
      //~ ubones[i] = getRotationRep(i);
      getRotationRep(i);
      
      Skeleton *si = model->i_skeleton; // initial skeleton
      Skeleton *sd = model->skeleton;   // deformed skeleton
      const Bone &bi = si->bones[i];   // initial bone
      const Bone &bd = sd->bones[i];   // deformed bone
      Point3 p0 = si->joints[bi.first]; 
      Point3 p1 = si->joints[bi.second];
      Point3 q0 = sd->joints[bd.first]; 
      Point3 q1 = sd->joints[bd.second];
      
      u[0] = ubones[i][0]; u[1] = ubones[i][1]; u[2] = ubones[i][2];
      computeRotation(&p0[0]);
      Vector3 t0 = q0 - Point3(rv[0],rv[1],rv[2]);
      Number nubone2 = ubones[i]*ubones[i];
      t0 = (t0 - cross_product(ubones[i],t0))/sqrt(1.0 + nubone2);
      computeRotation(&p1[0]);
      Vector3 t1 = q1 - Point3(rv[0],rv[1],rv[2]);
      t1 = (t1 - cross_product(ubones[i],t1))/sqrt(1.0 + nubone2);
      tvectors0[i] = t0;
      tvectors1[i] = t1;
      tscalar0[i] = t0*ubones[i];
      tscalar1[i] = t1*ubones[i];
      
      //~ cout<<"u: "<<ubones[i][0]<<" "<<ubones[i][1]<<" "<<ubones[i][2]<<" "<<endl;
      //~ cout<<"t0: "<<t0[0]<<" "<<t0[1]<<" "<<t0[2]<<" "<<endl;
      //~ cout<<"t1: "<<t1[0]<<" "<<t1[1]<<" "<<t1[2]<<" "<<endl;
      //~ cout<<"dots: "<<t0*ubones[i]<<" "<<t1*ubones[i]<<endl;
   }
   
   //~ cout<<"beginnnnnnnnnnnnnnnn endd"<<endl;
   
   
   unsigned int ii=0, iv=0;
   Vertex_iterator vi = model->vertices_begin();
   while (vi != model->vertices_end()) {
      vector<Vector3> vt(vi->bonesweights.size(), Vector3(0,0,0));
      vector<Number>  sc(vi->bonesweights.size(), 0);
      //~ cout<<"vid: "<<vi->id()/3<<endl;
      for (unsigned int i=0; i<vi->bonesweights.size(); ++i) {
         //~ if (vi->bonesweights[i] > 0) {
            //~ cout<<" ... t0: "<<tvectors0[i][0]<<" "<<tvectors0[i][1]<<" "<<tvectors0[i][2]<<endl;
            //~ cout<<" ... t1: "<<tvectors1[i][0]<<" "<<tvectors1[i][1]<<" "<<tvectors1[i][2]<<endl;
            vt[i] = (tvectors0[i])*(vi->ts[i]) + (tvectors1[i])*(1-vi->ts[i]);
            sc[i] = (tscalar0[i])*(vi->ts[i])  + (tscalar1[i])*(1-vi->ts[i]);
            //~ vt[i] = (tvectors1[i])*(vi->ts[i]) + (tvectors0[i])*(1-vi->ts[i]);
            //~ sc[i] = (tscalar1[i])*(vi->ts[i])  + (tscalar0[i])*(1-vi->ts[i]);
         //~ }
            //~ cout<<"   ibone: "<<i<<" t: "<<vi->ts[i]<<endl;
            //~ cout<<"   vt: "<<vt[i][0]<<" "<<vt[i][1]<<" "<<vt[i][2]<<endl;
            //~ cout<<"   sc: "<<sc[i]<<endl;
      }
      
      /// 
      Number s1 = sc[0];
      Number s2 = s1;
      Vector3 t1 = vt[0];
      Vector3 t2 = t1;
      Number  b1 = vi->bonesweights[0]; 
      Number  qs1 = sqrt(1.0/(1.0+(ubones[0])*(ubones[0])));
      Vector3 qv1 = (ubones[0])*qs1;
      Number  b2 = b1;
      Number  qs2 = qs1;
      Vector3 qv2 = qv1;
      
      for (unsigned i=0; i<vi->bonesweights.size(); ++i) {
         
         if (not vi->bonesweights[i] > 0) continue;
         
         qs2 = sqrt(1.0/(1.0+(ubones[i])*(ubones[i])));
         qv2 = (ubones[i])*qs2;
         t2 = vt[i];
         s2 = sc[i];
         Number sumtmp = b1 + vi->bonesweights[i];
         b1 = b1/sumtmp;
         b2 = (vi->bonesweights[i])/sumtmp;
         Number cosangle = qs2*qs1 + qv2*qv1;
         if (abs(cosangle) > 1.0-EPSILON) {
            t1 = t1*b1 + t2*b2;
            s1 = s1*b1 + s2*b2;
            b1 = sumtmp; continue;
         }
         if (cosangle < 0) {
            qs2 = -qs2;
            qv2 = qv2*(-1.0);
            cosangle = -cosangle;
         }
         Number angle = acos(cosangle);
         Number sinangle = sin(angle);
         Number coef1 = (1.0/sinangle)*sin(b1*angle);
         Number coef2 = (1.0/sinangle)*sin(b2*angle);
         qs1 = qs1*coef1 + qs2*coef2;
         qv1 = qv1*coef1 + qv2*coef2;
         t1 = t1*coef1 + t2*coef2;
         s1 = s1*coef1 + s2*coef2;
         b1 = sumtmp;
      }
      Vector3 tv = t1*qs1 - qv1*s1 + cross_product(qv1, t1);
      
      //~ cout<<"q: "<<qs1<<" "<<qv1[0]<<" "<<qv1[1]<<" "<<qv1[2]<<endl;
      //~ cout<<"t1: "<<t1[0]<<" "<<t1[1]<<" "<<t1[2]<<endl;
      //~ cout<<"tv: "<<tv[0]<<" "<<tv[1]<<" "<<tv[2]<<endl;
      
      // Rotate the vertex position
      computeQuatRotation(&ipositions[ii][0], qs1, qv1);
      Point3 &p = vi->point();
      p = Point3(qstar[0]+rv[0], qstar[1]+rv[1], qstar[2]+rv[2]);
      
      // Rotate the normal vector
      computeQuatRotation(&inormals[ii][0], qs1, qv1);
      Vector3 &n = vi->normal();
      n = Vector3(rv[0], rv[1], rv[2]);
      
      ++vi; ++ii; iv+=3;
   }
   
}

void DeformationSkelMesh::project_deformation_on_mesh_linear() {
   
   //~ cout<<"beginnnnnnnnnnnnnnnn"<<endl;
   
   //< Compute the transformation for skeleton bones 
   //< skip the first bone
   ubones[0] = Vector3(0,0,0);
   tvectors0[0] = Vector3(0,0,0);
   tvectors1[0] = Vector3(0,0,0);
   tscalar0[0] = 0;
   tscalar1[0] = 0;
   for (unsigned int i=1; i<model->nBones(); ++i) {
      
      //~ cout<<"i: "<<i<<endl;
      //~ ubones[i] = getRotationRep(i);
      getRotationRep(i);
      
      Skeleton *si = model->i_skeleton; // initial skeleton
      Skeleton *sd = model->skeleton;   // deformed skeleton
      const Bone &b = si->bones[i];   // initial bone
      Point3 p0 = si->joints[b.first]; 
      Point3 p1 = si->joints[b.second];
      Point3 q0 = sd->joints[b.first]; 
      Point3 q1 = sd->joints[b.second];
      
      u[0] = ubones[i][0]; u[1] = ubones[i][1]; u[2] = ubones[i][2];
      computeRotation(&p0[0]);
      Vector3 t0 = q0 - Point3(rv[0],rv[1],rv[2]);
      Number nubone2 = ubones[i]*ubones[i];
      //~ t0 = (t0 + cross_product(ubones[i],t0))/sqrt(1.0 + nubone2);
      computeRotation(&p1[0]);
      Vector3 t1 = q1 - Point3(rv[0],rv[1],rv[2]);
      //~ t1 = (t1 + cross_product(ubones[i],t1))/sqrt(1.0 + nubone2);
      tvectors0[i] = t0;
      tvectors1[i] = t1;
      //~ tscalar0[i] = t0*ubones[i];
      //~ tscalar1[i] = t1*ubones[i];
      
      //~ cout<<"u: "<<ubones[i][0]<<" "<<ubones[i][1]<<" "<<ubones[i][2]<<" "<<endl;
      //~ cout<<"t0: "<<t0[0]<<" "<<t0[1]<<" "<<t0[2]<<" "<<endl;
      //~ cout<<"t1: "<<t1[0]<<" "<<t1[1]<<" "<<t1[2]<<" "<<endl;
      //~ cout<<"dots: "<<t0*ubones[i]<<" "<<t1*ubones[i]<<endl;
   }
   
   //~ cout<<"beginnnnnnnnnnnnnnnn endd"<<endl;
   
   
   unsigned int ii=0, iv=0;
   Vertex_iterator vi = model->vertices_begin();
   while (vi != model->vertices_end()) {
      vector<Vector3> vt(vi->bonesweights.size(), Vector3(0,0,0));
      vector<Number>  sc(vi->bonesweights.size(), 0);
      Point3 po = model->getVertexPosition(vi);
      //~ cout<<"vid: "<<vi->id()/3<<" "<<po[0]<<" "<<po[1]<<" "<<po[2]<<endl;
      for (unsigned int i=0; i<vi->bonesweights.size(); ++i) {
         //~ if (not vi->bonesweights[i] > 0) continue;
            //~ cout<<" ... t0: "<<tvectors0[i][0]<<" "<<tvectors0[i][1]<<" "<<tvectors0[i][2]<<endl;
            //~ cout<<" ... t1: "<<tvectors1[i][0]<<" "<<tvectors1[i][1]<<" "<<tvectors1[i][2]<<endl;
            vt[i] = (tvectors0[i])*(vi->ts[i]) + (tvectors1[i])*(1-vi->ts[i]);
            //~ sc[i] = (tscalar0[i])*(vi->ts[i])  + (tscalar1[i])*(1-vi->ts[i]);
            //~ cout<<"   ibone: "<<i<<" t: "<<vi->ts[i]<<" peso "<<vi->bonesweights[i]<<endl;
            //~ cout<<"   vt: "<<vt[i][0]<<" "<<vt[i][1]<<" "<<vt[i][2]<<endl;
            //~ cout<<"   sc: "<<sc[i]<<endl;
      }
      
      /// 
      Number s1 = sc[0];
      Number s2 = s1;
      Vector3 t1 = vt[0];
      Vector3 t2 = t1;
      Number  b1 = vi->bonesweights[0]; 
      Number  qs1 = sqrt(1.0/(1.0+(ubones[0])*(ubones[0])));
      Vector3 qv1 = (ubones[0])*qs1;
      Number  b2 = b1;
      Number  qs2 = qs1;
      Vector3 qv2 = qv1;
      
      for (unsigned i=0; i<vi->bonesweights.size(); ++i) {
         if (not vi->bonesweights[i] > 0) continue;
         
         qs2 = sqrt(1.0/(1.0+(ubones[i])*(ubones[i])));
         qv2 = (ubones[i])*qs2;
         t2 = vt[i];
         s2 = sc[i];
         Number sumtmp = b1 + vi->bonesweights[i];
         b1 = b1/sumtmp;
         b2 = (vi->bonesweights[i])/sumtmp;
         Number cosangle = qs2*qs1 + qv2*qv1;
         if (abs(cosangle) > 1.0-EPSILON) {
            t1 = t1*b1 + t2*b2;
            s1 = s1*b1 + s2*b2;
            b1 = sumtmp; continue;
         }
         if (cosangle < 0) {
            qs2 = -qs2;
            qv2 = qv2*(-1.0);
            cosangle = -cosangle;
         }
         //~ Number angle = acos(cosangle);
         //~ Number sinangle = sin(angle);
         //~ Number coef1 = (1.0/sinangle)*sin(b1*angle);
         //~ Number coef2 = (1.0/sinangle)*sin(b2*angle);
         qs1 = qs1*b1 + qs2*b2;
         qv1 = qv1*b1 + qv2*b2;
         t1 = t1*b1 + t2*b2;
         b1 = sumtmp;
      }
      Number normaq = sqrt(qs1*qs1 + qv1*qv1);
      qs1 = qs1 / normaq;
      qv1 = qv1*(1.0/normaq);
      Vector3 tv = t1;
      
      //~ cout<<"q: "<<qs1<<" "<<qv1[0]<<" "<<qv1[1]<<" "<<qv1[2]<<endl;
      //~ cout<<"t1: "<<t1[0]<<" "<<t1[1]<<" "<<t1[2]<<endl;
      //~ cout<<"tv: "<<tv[0]<<" "<<tv[1]<<" "<<tv[2]<<endl;
      
      // Rotate the vertex position
      computeQuatRotation(&ipositions[ii][0], qs1, qv1);
      Point3 &p = vi->point();
      p = Point3(qstar[0]+rv[0], qstar[1]+rv[1], qstar[2]+rv[2]);
      
      // Rotate the normal vector
      computeQuatRotation(&inormals[ii][0], qs1, qv1);
      Vector3 &n = vi->normal();
      n = Vector3(rv[0], rv[1], rv[2]);
      
      ++vi; ++ii; iv+=3;
   }
   
}

void DeformationSkelMesh::project_deformation_on_mesh_std_dualquat() {
   
   //< skip the first bone
   ubones[0] = Vector3(0,0,0);
   tvectors0[0] = Vector3(0,0,0);
   tvectors1[0] = Vector3(0,0,0);
   tscalar0[0] = 0;
   tscalar1[0] = 0;
   
   tlvectors0[0] = Vector3(0,0,0);
   tlvectors1[0] = Vector3(0,0,0);
   
   for (unsigned int ii=1; ii<model->nBones(); ++ii) {
      
      int i = model->i_skeleton->orderList[ii];
      
      Vector3 oldu = ubones[i];
      comprotaround = true;
      getRotationRep(i); // compute the ubones[i]
      
      Vector3 urot(0,0,0);
      if (comprotaround) urot = getRotationAroundBoneRep(i);
      else              comprotaround = true;
      
      //~ if (abs(1.0-ubones[i]*urot)<1e-8) {
         //~ Vector3 unitu = (ubones[i]+urot+cross_product(ubones[i],urot));
         //~ Number nunitu = sqrt(unitu*unitu);
         //~ if (nunitu < 1e-8)
            //~ ubones[i] = oldu;
         //~ else {
            //~ unitu = unitu/nunitu;
            //~ ubones[i] = unitu*(SGN(oldu*unitu)*1e8);
         //~ }
      //~ }
      //~ else {
         //~ ubones[i] = (ubones[i]+urot+cross_product(ubones[i],urot))/(1-ubones[i]*urot);
      //~ }
      
      Number cosbonei = 1.0/sqrt(1+ubones[i]*ubones[i]);
      Vector3 vecbonei = ubones[i]*cosbonei;
      
      Number cosroti = 1.0/sqrt(1+urot*urot);
      Vector3 vecroti = urot*cosroti;
      
      Number cosprod = cosbonei*cosroti - vecbonei*vecroti;
      Vector3 vecprod = vecroti*cosbonei + vecbonei*cosroti + cross_product(vecbonei,vecroti);
      if (cosprod<0) { 
         cosprod = -cosprod;
         vecprod = vecprod*(-1);
      }
      
      if (cosprod < 0.01)  cosprod = 0.01;
      ubones[i] = vecprod/cosprod;
   }
   
   Skeleton *si = model->i_skeleton; // initial skeleton
   for (unsigned int i=1; i<model->nBones(); ++i) {
      
      Skeleton *si = model->i_skeleton; // initial skeleton
      Skeleton *sd = model->skeleton;   // deformed skeleton
      const Bone &b = si->bones[i];   // initial bone
      const Point3 &p0 = si->joints[b.first]; 
      const Point3 &p1 = si->joints[b.second];
      const Point3 &q0 = sd->joints[b.first]; 
      const Point3 &q1 = sd->joints[b.second];
      
      u[0] = ubones[i][0]; u[1] = ubones[i][1]; u[2] = ubones[i][2];
      computeRotation(&p0[0]);
      
      Vector3 t0 = q0 - Point3(rv[0],rv[1],rv[2]);
      Number nubone2 = ubones[i]*ubones[i];
      Number denom = 1.0/sqrt(1.0 + nubone2);
      Vector3 utemp = ubones[i]*denom;
      tscalar0[i] = -1.0*(t0*utemp);
      tlvectors0[i] = t0;
      tvectors0[i] = (t0*denom - cross_product(utemp,t0));
      computeRotation(&p1[0]);
      
      Vector3 t1 = q1 - Point3(rv[0],rv[1],rv[2]);
      tscalar1[i] = -1.0*(t1*utemp);
      tlvectors1[i] = t1;
      tvectors1[i] = (t1*denom - cross_product(utemp,t1));
   }
   
   //~ cout<<"projectando ... "<<endl;
   /// on vertices
   unsigned int ii=0, iv=0;
   Vertex_iterator vi = model->vertices_begin();
   while (vi != model->vertices_end()) {
      vector<Vector3> vt(vi->bonesweights.size(), Vector3(0,0,0));
      vector<Number> sc(vi->bonesweights.size(), 0);
      Point3 po = model->getVertexPosition(vi);
      
      Number sumweights = 0;
      
      Number s1 = 0;
      Vector3 t1(0,0,0);
      Number qs1 = 0;
      Vector3 qv1(0,0,0);
      
      Vector3 vtl(0,0,0);
      
      Vector3 u1(0,0,0);
      Vector3 vmod(0,0,0); // position
      Vector3 nmod(0,0,0); // normal
      for (unsigned int i=1; i<vi->bonesweights.size(); ++i) {
         if (vi->bonesweights[i] == 0) continue;
         
         vtl = (tlvectors0[i])*(vi->ts[i]) + (tlvectors1[i])*(1.0-vi->ts[i]);
         vt[i] = (tvectors0[i])*(vi->ts[i]) + (tvectors1[i])*(1.0-vi->ts[i]);
         sc[i] = (tscalar0[i])*(vi->ts[i])  + (tscalar1[i])*(1.0-vi->ts[i]);
         /// usando u
         
         u[0]=ubones[i][0]; u[1]=ubones[i][1]; u[2]=ubones[i][2];
         computeRotation(&ipositions[ii][0]); 
         vmod = vmod + (Vector3(rv[0],rv[1],rv[2])+vtl)*vi->bonesweights[i];
         
         computeRotation(&inormals[ii][0]); 
         nmod = nmod + (Vector3(rv[0],rv[1],rv[2]))*vi->bonesweights[i];
         
         /// 
         
         Number cosi = (sqrt(1.0/(1.0+(ubones[i])*(ubones[i])))); //turns[i];
         Vector3 vecparti = ubones[i]*cosi;
         
         if ((qs1*cosi + qv1*vecparti)<0) {
            cosi = -cosi;
            vecparti = -vecparti;
            vt[i] = vt[i]*(-1);
            sc[i] = sc[i]*(-1);
            //~ cout<<"changed ..."<<endl;
         }
         s1 += sc[i]*vi->bonesweights[i];
         t1 = t1 + vt[i]*vi->bonesweights[i];
         qs1 += cosi*vi->bonesweights[i];
         qv1 = qv1 + (vecparti)*vi->bonesweights[i];
         
         sumweights += vi->bonesweights[i];
      }
      
      assert(sumweights != 0);
      
      
      vmod = vmod/sumweights;
      nmod = nmod/sumweights;
      
      s1 /= sumweights;
      t1 = t1/sumweights;
      
      qs1 /= sumweights;
      qv1 = qv1/sumweights;
      
      //~ u[0] = u1[0]/sumweights; u[1] = u1[1]/sumweights; u[2] = u1[2]/sumweights;
      
      //~ if (t1*t1 > 2) {
         //~ cout<<"t1: "<<t1<<" : "<<qv1<<" : "<<qs1<<endl;
      //~ }
      
      
      Number normaq = sqrt(qs1*qs1 + qv1*qv1);
      //~ assert(normaq != 0);
      //~ if (qs1<0) normaq = -normaq;
      qs1 = qs1/normaq;
      qv1 = qv1*(1.0/normaq);
      
      t1 = t1/normaq;
      s1 = s1/normaq;
      
      Vector3 tv = t1*qs1 - qv1*s1 + cross_product(qv1, t1);
      
      //~ Vector3 tv = t1;
      
      if (qs1 < 0.001) qs1 = 0.001;
      u[0] = qv1[0]*(1.0/qs1); u[1] = qv1[1]*(1.0/qs1); u[2] = qv1[2]*(1.0/qs1);
      
      //~ /// Perform the rotation of v
      
      Number Lambda = 1.0; 
      
      // Rotate the vertex position
      computeRotation(&ipositions[ii][0]);
      Point3 &p = vi->point();
      p = Point3(tv[0]+rv[0], tv[1]+rv[1], tv[2]+rv[2]);
/*      p = Point3((tv[0]+rv[0])*Lambda, (tv[1]+rv[1])*Lambda, (tv[2]+rv[2])*Lambda);
      //~ p = Point3(vmod[0],vmod[1],vmod[2]);
      vmod = vmod*(1-Lambda);
      Point3 p2 = Point3(vmod[0],vmod[1],vmod[2]);
      p = Point3(p[0]+p2[0],p[1]+p2[1],p[2]+p2[2]); */
      
      //~ if (p.x() < -1) {
         //~ cout<<t1<<" : "<<qv1<<" : "<<qs1<<" : "<<s1<<endl;
         //~ cout<<"tv: "<<tv<<" :: "<<p<<endl;
      //~ }
      
      
      // Rotate the normal vector
      //~ computeRotation(&inormals[ii][0]);
      //~ Vector3 &n = vi->normal();
      //~ n = Vector3(rv[0], rv[1], rv[2]);
      //~ n = Vector3(nmod[0], nmod[1], nmod[2]);
      
      ++vi; ++ii; iv+=3;
   }
}

Number omega1(Number d) {
   
   if (d==NINFINITY) { return 0.00000001; }
   if (d==0)  d = 0.00000001;
   return 1.0/(pow(d,2));
}

void DeformationSkelMesh::noproject_deformation() {
   
   
   Number EPSILON = 1.0e-6;
   // Deform the vertices of the model
   Vertex_iterator vi = model->vertices_begin(); unsigned int ii=0, iv=0;
   while (vi != model->vertices_end()) {
      u[0]=u[1]=u[2]=qstar[0]=qstar[1]=qstar[2]=0.0;
      
      const unsigned int &g = vi->rigging_group;
      const Number &dg = vi->dClosestJoint;
      Point3 po = model->getVertexPosition(vi);
      
      Wt=pstar[0]=pstar[1]=pstar[2]=0.0;
      Wt=qstar[0]=qstar[1]=qstar[2]=0.0;
      for (unsigned j=0; j<nconstraints; ++j) {
         //~ const Point3 &P = &(i_constraintsPos)[j];
         //~ const Number &Q = &(*d_constraintsPos)[j][0]; 
         const Point3 &P = (*i_constraintsPos)[j];
         const Point3 &Q = (*d_constraintsPos)[j];
         Vector3 dd = po - Point3(P[0],P[1],P[2]);
         //~ Number d = sqrt(dd*dd);
         const Number d = distance(g,j) + dg;
         
         W = omega1(d);
         Wt += W;
         pstar[0] += W*P[0]; pstar[1] += W*P[1]; pstar[2] += W*P[2];
         qstar[0] += W*Q[0]; qstar[1] += W*Q[1]; qstar[2] += W*Q[2];
      }
      pstar[0] /= Wt; pstar[1] /= Wt; pstar[2] /= Wt;
      qstar[0] /= Wt; qstar[1] /= Wt; qstar[2] /= Wt;
      
      //~ computeCorrelationMatrix_points(i); 
      {
      Mf0[0]=Mf0[1]=Mf0[2]=
      Mf1[0]=Mf1[1]=Mf1[2]=
      Mf2[0]=Mf2[1]=Mf2[2]=0.0;
      //~ const float *pihatV = &phatTable[g*3]; /// pointer to phat of vertex i
      unsigned tsize=3*texSizetexSize;
      
      for (unsigned j=0, row=0; j<nconstraints; ++j, row+=tsize) {
         //~ const float *pihat = &pihatV[row];
         //~ const Number *P = &(i_constraintsPos)[j][0];
         //~ const Number *Q = &(*d_constraintsPos)[j][0];
         const Point3 &P = (*i_constraintsPos)[j];
         const Point3 &Q = (*d_constraintsPos)[j];
         Vector3 dd = po - Point3(P[0],P[1],P[2]);
         Number d = sqrt(dd*dd);
         //~ const Number d = distance(g,j) + dg;
         W = omega1(d);
         
         qjhat[0] = W*(Q[0]-qstar[0]); 
         qjhat[1] = W*(Q[1]-qstar[1]); 
         qjhat[2] = W*(Q[2]-qstar[2]);
         
         Vector3 pihat(P[0]-pstar[0], P[1]-pstar[1], P[2]-pstar[2]);
         
         Mf0[0]+=qjhat[0]*pihat[0]; Mf0[1]+=qjhat[0]*pihat[1]; Mf0[2]+=qjhat[0]*pihat[2];
         Mf1[0]+=qjhat[1]*pihat[0]; Mf1[1]+=qjhat[1]*pihat[1]; Mf1[2]+=qjhat[1]*pihat[2];
         Mf2[0]+=qjhat[2]*pihat[0]; Mf2[1]+=qjhat[2]*pihat[1]; Mf2[2]+=qjhat[2]*pihat[2];
      }
      
      detM = Mf0[0]*(Mf1[1]*Mf2[2]-Mf1[2]*Mf2[1])+
             Mf0[1]*(Mf1[2]*Mf2[0]-Mf1[0]*Mf2[2])+
             Mf0[2]*(Mf1[0]*Mf2[1]-Mf2[0]*Mf1[1]);
      traceM = Mf0[0] + Mf1[1] + Mf2[2];
      V[0]=Mf1[2]-Mf2[1]; V[1]=Mf2[0]-Mf0[2]; V[2]=Mf0[1]-Mf1[0];
      }
      
      eigenvalue = computeLargestRootOfQuarticEquation() + traceM;
      compute_u();
      
      // Rotate the vertex position
      computeRotation(&vhatList[ii][0]);
      Point3 &p = vi->point();
      p = Point3(qstar[0]+rv[0], qstar[1]+rv[1], qstar[2]+rv[2]);
      
      // Rotate the normal vector
      computeRotation(&inormals[ii][0]);
      Vector3 &n = vi->normal();
      n = Vector3(rv[0], rv[1], rv[2]);
      
      ++vi; ++ii; iv+=3;
   }
   
}

//~ }
